/*
Copyright 2013 Lin, Chung-Ming (aka Alva Lin. E-mail: alva0930@gmail.com)

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
import java.util.*;

/**
 * A Java translation of <a href=http://sprinkler.googlecode.com/svn/trunk/regression/fmincg.m>fmincg.m</a>
 * <P>
 * The maximum number of iterations is restricted to a positive number.
 * Zero or negative number of iterations will cause an {@link AssertionError} when assertions are enabled,
 * or cause unexpected result when assertions are disabled.
 * <P>
 * <DL>
 * <DT><B>Sample Code:</B></DT>
 * <DD>Fmincg fmincg = new Fmincg();</DD>
 * <DD>{@link CostGradient} cg = &ltan implementation of CostGradient&gt;</DD>
 * <DD>double[] init_params = &ltinitial parameters&gt;</DD>
 * <DD>int maxIter = &ltmaximum number of iterations in fmincg, <B>must be positive</B>&gt;</DD>
 * <DD>fmincg.minimize(cg, init_params, maxIter);</DD>
 * <DD>double[] cost_minimized_parameters = fmincg.getParameter();</DD>
 * <DD>double[] cost_after_each_iteration = fmincg.getCost();</DD>
 * </DL>
 *
 * @see	CostGradient
 * @author	Lin, Chung-Ming (aka Alva Lin. E-mail: alva0930@gmail.com)
 */
public class Fmincg
{
	private static final double RHO = 0.01;																						// a bunch of constants for line searches
	private static final double SIG = 0.5;																						// RHO and SIG are the constants in the Wolfe-Powell conditions
	private static final double INT = 0.1;																						// don't reevaluate within 0.1 of the limit of the current bracket
	private static final double EXT = 3.0;																						// extrapolate maximum 3 times the current bracket
	private static final int MAX = 20;																								// max 20 function evaluations per line search
	private static final double RATIO = 100;																					// maximum allowed slope ratio

	private double[] X;
	private ArrayList<Double> fX;

/* ------------------------
   Public Methods
 * ------------------------ */

	/**
	 * Get the costs after each iteration of last invocation of minimize(...).
	 *
	 * @return	the costs after each iteration of last invocation of minimize(...), or null if minimize(...) has never been invoked
	 */
	public double[] getCost()
	{
		if (null == fX) {return null;}
		double[] result = new double[fX.size()];
		for (int i = 0; i < fX.size(); i++) {result[i] = fX.get(i);}
		return result;
	}

	/**
	 * Get the cost after last iteration of last invocation of minimize(...).
	 *
	 * @return	the cost after last iteration of last invocation of minimize(...), or {@link Double}.POSITIVE_INFINITY if minimize(...) has never been invoked
	 */
	public double getLastCost()
	{
		if (null == fX) {return Double.POSITIVE_INFINITY;}
		return fX.get(fX.size() - 1);
	}

	/**
	 * Get the cost-minimized parameters of last invocation of minimize(...).
	 *
	 * @return	the cost-minimized parameters of last invocation of minimize(...), or null if minimize(...) has never been invoked
	 */
	public double[] getParameter()
	{
		return X;
	}

	/**
	 * Minimize a continuous differentiable multivariate function.
	 * The Polack-Ribiere flavour of conjugate gradients is used to compute search directions,
	 * and a line search using quadratic and cubic polynomial approximations and the Wolfe-Powell stopping criteria is used
	 * together with the slope ratio method for guessing initial step sizes.
	 *
	 * @param	f				an implementation of {@link CostGradient} used to evaluate the cost and the partial derivatives for a given set of parameters
	 * @param	X_input	the initial parameters
	 * @param	MaxIter	the maximum number of line searches, must be positive
	 */
	public void minimize(CostGradient f, double[] X_input, int MaxIter)
	{
		assert(MaxIter > 0);

		X = Arrays.copyOf(X_input, X_input.length);
		int length = MaxIter;
		double red = 1.0;

		int i = 0;																																			// zero the run length counter
		boolean ls_failed = false;																											// no previous line search has failed
		fX = new ArrayList<Double>();
		f.evaluate(X);																																	// get function value and gradient
		double f1 = f.getCost();
		double[] df1 = f.getGradient();
		double[] s = new double[df1.length];																						// search direction is steepest
		for (int idx = 0; idx < s.length; idx++) {s[idx] = (-df1[idx]);}
		double d1 = 0;																																	// this is the slope
		for (int idx = 0; idx < s.length; idx++) {d1 += (-s[idx]) * s[idx];}
		double z1 = red / (1.0 - d1);																										// initial step is red/(|s|+1)

		while (i < length)																															// while not finished
		{
			i++;
			double[] X0 = Arrays.copyOf(X, X.length);																			// make a copy of current values
			double f0 = f1;
			double[] df0 = Arrays.copyOf(df1, df1.length);
			for (int idx = 0; idx < X.length; idx++) {X[idx] += z1 * s[idx];}							// begin line search
			f.evaluate(X);
			double f2 = f.getCost();
			double[] df2 = f.getGradient();
			double d2 = 0; for (int idx = 0; idx < df2.length; idx++) {d2 += df2[idx] * s[idx];}
			double f3 = f1;																																// initialize point 3 equal to point 1
			double d3 = d1;
			double z3 = (-z1);
			int M = MAX;
			boolean success = false;																											// initialize quanteties
			double limit = (-1.0);

			while (true)
			{
				while (((f2 > f1 + z1 * RHO * d1) || (d2 > (-SIG) * d1)) && (M > 0))
				{
					limit = z1;																																// tighten the bracket
					double z2 = 0;
					if (f2 > f1)
					{z2 = z3 - (0.5 * d3 * z3 * z3) / (d3 * z3 + f2 - f3);}										// quadratic fit
					else
					{
						double A = 6 * (f2 - f3) / z3 + 3 * (d2 + d3);													// cubic fit
						double B = 3 * (f3 - f2) - z3 * (d3 + 2 * d2);
						z2 = (Math.sqrt(B * B - A * d2 * z3 * z3) - B) / A;											// numerical error possible - ok!
					}
					if (Double.isNaN(z2) || Double.isInfinite(z2))
					{z2 = z3 / 2;}																														// if we had a numerical problem then bisect
					z2 = Math.max(Math.min(z2, INT * z3), (1 - INT) * z3);										// don't accept too close to limits
					z1 = z1 + z2;																															// update the step
					for (int idx = 0; idx < X.length; idx++) {X[idx] += z2 * s[idx];}
					f.evaluate(X); f2 = f.getCost(); df2 = f.getGradient();
					M = M - 1;
					d2 = 0; for (int idx = 0; idx < df2.length; idx++) {d2 += df2[idx] * s[idx];}
					z3 = z3 - z2;																															// z3 is now relative to the location of z2
 				}

 				if (f2 > f1 + z1 * RHO * d1 || d2 > (-SIG) * d1)
 				{break;}																																		// this is a failure
 				else if (d2 > SIG * d1)
 				{success = true; break;}																										// success
 				else if (M == 0)
 				{break;}																																		// failure

				double A = 6 * (f2 - f3) / z3 + 3 * (d2 + d3);															// make cubic extrapolation
				double B = 3 * (f3 - f2) - z3 * (d3 + 2 * d2);
				double z2 = (-d2) * z3 * z3 / (B + Math.sqrt(B * B - A * d2 * z3 * z3));		// num. error possible - ok!

				if (Double.isNaN(z2) || Double.isInfinite(z2) || z2 < 0)										// num prob or wrong sign?
				{
					if (limit < (-0.5))																												// if we have no upper limit
					{z2 = z1 * (EXT - 1);}																										// the extrapolate the maximum amount
					else
					{z2 = (limit - z1) / 2;}																									// otherwise bisect
				}
				else if ((limit > (-0.5)) && ((z2 + z1) > limit))														// extraplation beyond max?
				{z2 = (limit - z1) / 2;}																										// bisect
				else if ((limit < (-0.5)) && ((z2 + z1) > (z1 * EXT)))											// extrapolation beyond limit
				{z2 = z1 * (EXT - 1.0);}																										// set to extrapolation limit
				else if (z2 < ((-z3) * INT))
				{z2 = (-z3) * INT;}
				else if ((limit > (-0.5)) && (z2 < (limit - z1) * (1.0 - INT)))							// too close to limit?
				{z2 = (limit - z1) * (1.0 - INT);}

				f3 = f2; d3 = d2; z3 = (-z2);																								// set point 3 equal to point 2
				z1 = z1 + z2;
				for (int idx = 0; idx < X.length; idx++) {X[idx] += z2 * s[idx];}						// update current estimates
				f.evaluate(X); f2 = f.getCost(); df2 = f.getGradient();
				M = M - 1;
				d2 = 0; for (int idx = 0; idx < df2.length; idx++) {d2 += df2[idx] * s[idx];}
			}

			if (success)																																	// if line search succeeded
			{
				f1 = f2; fX.add(f1);
				/*    fprintf('%s %4i | Cost: %4.6e\r', S, i, f1); */
				double df2df2 = 0; for (int idx = 0; idx < df2.length; idx++) {df2df2 += df2[idx] * df2[idx];}
				double df1df2 = 0; for (int idx = 0; idx < df1.length; idx++) {df1df2 += df1[idx] * df2[idx];}
				double df1df1 = 0; for (int idx = 0; idx < df1.length; idx++) {df1df1 += df1[idx] * df1[idx];}
				double mul = (df2df2 - df1df2) / (df1df1);
				for (int idx = 0; idx < s.length; idx++) {s[idx] = mul * s[idx] - df2[idx];}// Polack-Ribiere direction
				double[] tmp = df1; df1 = df2; df2 = tmp;																		// swap derivatives
				d2 = 0; for (int idx = 0; idx < df1.length; idx++) {d2 += df1[idx] * s[idx];}
				if (d2 > 0)																																	// new slope must be negative
				{
					for (int idx = 0; idx < s.length; idx++) {s[idx] = (-df1[idx]);}					// otherwise use steepest direction
					d2 = 0; for (int idx = 0; idx < s.length; idx++) {d2 += (-s[idx]) * s[idx];}
				}
				z1 *= Math.min(RATIO, d1 / (d2 - Double.MIN_VALUE));												// slope ratio but max RATIO
				d1 = d2;
				ls_failed = false;																													// this line search did not fail
			}
			else
			{
				X = X0; f1 = f0; df1 = df0;																									// restore point from before failed line search
				if (ls_failed) {break;}																											// line search failed twice in a row, give up
				double[] tmp = df1; df1 = df2; df2 = tmp;																		// swap derivatives
				for (int idx = 0; idx < s.length; idx++) {s[idx] = (-df1[idx]);}						// try steepest
				d1 = 0; for (int idx = 0; idx < s.length; idx++) {d1 += (-s[idx]) * s[idx];}
				z1 = 1 / (1 - d1);
				ls_failed = true;																														// this line search failed
			}
		}
	}
}